#include "star_align.h"

#include "dwarf_utils.h"
#include "elog.h"
#include "elog_file.h"

using namespace std;

int StarDetection(const Mat& imgSrc) {
	if(imgSrc.empty())
		return -10;

	unsigned int width = 0, height = 0;
	Mat imgGray;
	unsigned int threshold = 0;

	int i = 0;
	int noneZeroPix = 0;

	width = imgSrc.cols;
	height = imgSrc.rows;
 
	if(imgSrc.channels() == 1)
		imgGray = imgSrc.clone();
	else
		cvtColor(imgSrc, imgGray, COLOR_RGB2GRAY);

	Scalar mean;  
    Scalar dev;
	meanStdDev(imgGray, mean, dev);
	threshold = (unsigned int)(mean.val[0] + 3.0 * dev.val[0]) > 254 ? 254 : (unsigned int)(mean.val[0] + 3.0 * dev.val[0]);
	// log_i("threshold:%d", threshold);

  	//Setup a threshold 
	cv::threshold(imgGray, imgGray, threshold, 0, THRESH_TOZERO);

	if(threshold < 254 ) {
		double scale = 255.0 / (255.0 - threshold - 1);
		double shift = -255.0 * (threshold + 1) / (255.0 - threshold - 1);
		imgGray.convertTo(imgGray, CV_8U, scale, shift);
	}

	float SNR = 0;
	meanStdDev(imgGray, mean, dev);
	if(dev.val[0] > 1e-6)
		SNR = (float)(mean.val[0] / dev.val[0]);
	// log_i("SNR:%f", SNR);
	
	if(SNR >= 0.1) {
		log_i("cloud found!");
		return -1;
	}

	//椭圆卷积核心，开运算，去除噪点，圆滑星点边界
	Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(3, 3), Point(-1, -1));
	morphologyEx(imgGray, imgGray, MORPH_OPEN, kernel);

	vector<vector<Point>> contours;
	findContours(imgGray, contours, noArray(), RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

	unsigned int considerObjectsCount = 0;
	unsigned int starCount = 0;

	// log_i("objects found:%d", (unsigned int)contours.size());

	for (int k = 0; k < contours.size(); k++) {
		double area_1 = contourArea(contours[k]);
		if (area_1 < 5 || area_1 > 1000)  
			continue;
		considerObjectsCount++;

		Rect bRect = boundingRect(contours[k]);
		if(abs(bRect.width - bRect.height) >= 5)
			continue;

		starCount++;
	}
	// log_i("Max contour area:%f", areaMax);
	if(considerObjectsCount != 0) {
		if(considerObjectsCount < 10) {
			log_i("star not enough");
			return -2;
		}
		float starRatio = (float)starCount / (float)considerObjectsCount;
		// log_i("starCount:%d, considerObjectsCount:%d, starRatio:%f", starCount, considerObjectsCount, starRatio);
		if(starRatio < 0.3) {
			log_i("star line detected!");
			return -3;
		}
	}
	else {
		log_i("No star found!");
		return -4;
	}

	return 0;
}

//TODO: 4K叠图参数可能没调
int FindAndClassifyStars(const Mat& imgSrc, imgObject **starObjects, unsigned int *starCountMain) {
	if(imgSrc.empty())
		return -10;

	unsigned int width = 0, height = 0;
	unsigned int imageSize = 0;
	unsigned int pixelsFound = 0;
	unsigned int starCount = 0;
	Mat imgGray;
	pixel *image = NULL;
	pixel *imageReduced = NULL;
	unsigned int threshold = 0;

	int i = 0;
	int noneZeroPix = 0;

	width = imgSrc.cols;
	height = imgSrc.rows;
 
	if(imgSrc.channels() == 1)
		imgGray = imgSrc.clone();
	else
		cvtColor(imgSrc, imgGray, COLOR_BGR2GRAY);

	// Mat dstHist;
	// float hranges[] ={0, 256};			// 取值范围[0,256)
	// const float *ranges[] = {hranges};
	// int size=256;
	// int channels = 0;
	
	// //计算图像的直方图
	// calcHist(&imgGray, 1, &channels, Mat(), dstHist, 1, &size, ranges);
	// for(i = 255; noneZeroPix < 65535 && i > -1; i--)
	// 	noneZeroPix += dstHist.at<float>(i);
	// threshold = i + 1 > 254 ? 254 : i + 1;
	// log_i("threshold:%d", threshold);

	Scalar mean;  
    Scalar dev;
	meanStdDev(imgGray, mean, dev);
	threshold = (unsigned int)(mean.val[0] + 3.0 * dev.val[0]) > 254 ? 254 : (unsigned int)(mean.val[0] + 3.0 * dev.val[0]);
	// log_i("threshold:%d", threshold);

  	//Setup a threshold 
	cv::threshold(imgGray, imgGray, threshold, 0, THRESH_TOZERO);

	if(threshold < 254 ) {
		double scale = 255.0 / (255.0 - threshold - 1);
		double shift = -255.0 * (threshold + 1) / (255.0 - threshold - 1);
		imgGray.convertTo(imgGray, CV_8U, scale, shift);
	}

	float SNR = 0;
	meanStdDev(imgGray, mean, dev);
	if(dev.val[0] > 1e-6)
		SNR = (float)(mean.val[0] / dev.val[0]);
	// log_i("SNR:%f", SNR);
	
	if(SNR >= 0.1) {
		log_i("cloud found!");
		return -1;
	}

	//椭圆卷积核心，开运算，去除噪点，圆滑星点边界
	Mat kernel = getStructuringElement(MORPH_ELLIPSE, Size(3, 3), Point(-1, -1));
	morphologyEx(imgGray, imgGray, MORPH_OPEN, kernel);

	// log_i("\t\tStar line check...");
	int rev = ContourClassifyObjects(starObjects, starCountMain, imgGray);
	if(rev < 0)
		return rev;
	// log_i("stars found %d", *starCountMain);

	//Find the score and sort them
	// log_i("\t\tRanking objects...");
	RankObjects(*starObjects, *starCountMain);
	return 0;
}

unsigned int TriangulateFeatures(imgObject *objectSet, unsigned int starCount, triangleFeature *triangle) {
	unsigned int i_start = 1;
	unsigned int j_start = 2;
	unsigned int k_start = 3;
	unsigned int j_mid = j_start;
	unsigned int k_mid = k_start;
	unsigned int i,j,k;
	unsigned int index = 0;

	if(starCount < 3) {
		return 0;
	}
	for(i = i_start; i<=starCount-2; i++) {
		for(j = j_mid; j<=starCount-1; j++) {
			for(k = k_mid; k<=starCount; k++) {
				
				triangle[index].obj[0] = &objectSet[i-1];	
				triangle[index].obj[1] = &objectSet[j-1];	
				triangle[index].obj[2] = &objectSet[k-1];	
				
				triangle[index].angle[0] = FindAngle(objectSet[i-1].x, objectSet[i-1].y, objectSet[j-1].x, objectSet[j-1].y, objectSet[k-1].x, objectSet[k-1].y); 			
				triangle[index].angle[1] = FindAngle(objectSet[j-1].x, objectSet[j-1].y, objectSet[i-1].x, objectSet[i-1].y, objectSet[k-1].x, objectSet[k-1].y);				
				triangle[index].angle[2] = FindAngle(objectSet[k-1].x, objectSet[k-1].y, objectSet[i-1].x, objectSet[i-1].y, objectSet[j-1].x, objectSet[j-1].y); 				
				triangle[index].metric = sqrt(pow(triangle[index].angle[0],2)+pow(triangle[index].angle[1],2)+pow(triangle[index].angle[2],2)); 
				
				triangle[index].index[0] = i;			
				triangle[index].index[1] = j;			
				triangle[index].index[2] = k;			
//				log_i("%d\t%d\t%d\t%.3f\t%.3f\t%.3f\t", i,j,k,triangle[index].angle1,triangle[index].angle2,triangle[index].angle3);
//				log_i("metric:%f",triangle[index].metric);
				
				index++;
			}
			k_mid++;
		}
		j_mid++;
		k_start++;
		k_mid = k_start;
	}
	return index;
}

// Find the angle between three points
float FindAngle(float x1, float y1, float x2, float y2, float x3, float y3) {

	float unitV1[2];
	float unitV2[2];
	float mag1, mag2;

	unitV1[0] = x2-x1;
	unitV1[1] = y2-y1;

	unitV2[0] = x3-x1;
	unitV2[1] = y3-y1;
	
	return acos(UnitDotProduct(unitV1[0],unitV1[1],unitV2[0],unitV2[1]));	
}	

// Find the angle between three points
float UnitDotProduct(float V1i, float V1j, float V2i, float V2j) {

	float mag1, mag2;
	float unitV1i, unitV1j, unitV2i, unitV2j;

	mag1 = sqrt(pow(V1i,2) + pow(V1j,2));
	mag2 = sqrt(pow(V2i,2) + pow(V2j,2));

	unitV1i		= V1i/mag1;
	unitV1j		= V1j/mag1;
	unitV2i		= V2i/mag2;
	unitV2j		= V2j/mag2;
	
	return fminf(fmaxf((unitV1i*unitV2i+unitV1j*unitV2j), -1.0f), 1.0f);
	
}

int	CrossProduct(float v1i, float v1j, float v2i, float v2j)	{
	// | i 		j 		k |
	// | v1i	v1j		0 |
	// | v2i	v2j		0 |
	// result = k * ((v1i*v2j) - (v1j*v2i))

	return (v1i*v2j)-(v1j*v2i);	
}

float Mag(float a, float b)	{
	return sqrt(pow(a,2)+(pow(b,2)));
}

char MatchObjects(triangleFeature *triangleSet1, triangleFeature *triangleSet2, unsigned int triangleSet1Size, unsigned int triangleSet2Size, float metricCriteria, float angleCriteria, float minAngle, unsigned int *matchIndexI, unsigned int *matchIndexJ, possibleMatch *possibleMatches, unsigned int *matches) {
	unsigned int i, j, m, n;
	float 	minValue = 100.0;
	float	angleA, angleB;
	float	minAngleDiff[3] = {100.0, 100.0, 100.0};
	signed char	objMatchTemp[3] = {-1, -1, -1};

	for(i=0; i<triangleSet1Size; i++) {
		for(j=0; j<triangleSet2Size; j++) {
			//Check the metric
			/*(fabs(triangleSet1[i].metric-triangleSet2[j].metric) < minValue) && */
			if((fabs(triangleSet1[i].metric-triangleSet2[j].metric) < metricCriteria)) {
				//Make sure its an actual triangle and not a straight line
				if(triangleSet1[i].angle[0] > minAngle && triangleSet1[i].angle[1] > minAngle && triangleSet1[i].angle[2] > minAngle) {
					//If it passes the metric, is an actual triangle (not straight line), then lets check all the angles and see if they agree
					for(m=0; m<3; m++){
						for(n=0;n<3;n++) {
							angleA = triangleSet1[i].angle[m];
							angleB = triangleSet2[j].angle[n];
							if(fabs(angleA-angleB) < minAngleDiff[m]) {
								minAngleDiff[m] = fabs(angleA-angleB);
								triangleSet1[i].objMatch[m] = n;
								triangleSet2[j].objMatch[m] = n;		
							}						
//						log_i("AngleA = %f\tAngleB = %f\tminAngleDiff[%d] = %f",angleA,angleB,m,minAngleDiff[m]);
						}
					}
				}
				//If the angles are correct to within x angleCriteria make them the current most likely match
				if((minAngleDiff[0] < angleCriteria) && (minAngleDiff[1] < angleCriteria) && (minAngleDiff[2] < angleCriteria)) {
//					log_i("minAng[0]:%f minAng[1]:%f minAng[2]:%f",minAngleDiff[0],minAngleDiff[1],minAngleDiff[2]);


					//Perform the mass/angle check to ensure each angle corresponds with the proper mass size
					if(CheckMassOrder(triangleSet1[i], triangleSet2[j])) {	
						minValue = fabs(triangleSet1[i].metric-triangleSet2[j].metric);
						*matchIndexI = i;
						*matchIndexJ = j;
	//					log_i("Min value: %f", minValue);
						//Save the possible valid match

						if(*matches == triangleSet1Size) {
							log_i("Possible matches overflow!");
							break;
						}
						possibleMatches[(*matches)].tri1 = &triangleSet1[i];
						possibleMatches[(*matches)].tri2 = &triangleSet2[j];
						(*matches)++;
					}
				}
				minAngleDiff[0] = 100.0; 
				minAngleDiff[1] = 100.0; 
				minAngleDiff[2] = 100.0; 
			}
/*			
			if(fabs(triangleSet1[i].metric-triangleSet2[j].metric) < metricCriteria) {
				log_i("metricCriteria met: %f", fabs(triangleSet1[i].metric-triangleSet2[j].metric));
			}
			log_i("%.3f\t%.3f\t%.3f\t", triangleSet1[i].angle[0],triangleSet1[i].angle[1],triangleSet1[i].angle[2]);
			log_i("metric:%f",triangleSet1[i].metric);
			log_i("%.3f\t%.3f\t%.3f\t", triangleSet2[j].angle[0],triangleSet2[j].angle[1],triangleSet2[j].angle[2]);
			log_i("metric:%f",triangleSet2[j].metric);
			log_i("diff: %f",	fabs(triangleSet1[i].metric - triangleSet2[j].metric));
*/
		}
	}


	//log_i("Matches: %d", matches);
	if(minValue < metricCriteria) {
		return 1; 
	}

	return 0;
}

char CheckMassOrder(triangleFeature triangle1, triangleFeature triangle2) {
	unsigned int i, j;
	unsigned int mass1[3] = {0,0,0};
	unsigned int mass2[3] = {0,0,0};
	unsigned int tempMass;
	char		 massOrder1[3] = {0,1,2};
	char		 massOrder2[3] = {0,1,2};
	char		 tempOrder;
	
	for(i=0;i<3;i++){
		mass1[i] = triangle1.obj[i]->mass;
		mass2[i] = triangle2.obj[i]->mass; 
	}	

	//sort them and keep track
	for(i=0;i<3;i++) {
		for(j=0;j<3;j++) {
			if(mass1[i]>mass1[j]) {
				tempMass = mass1[i];
				mass1[i] = mass1[j];
				mass1[j] = tempMass;
				tempOrder = massOrder1[i];
				massOrder1[i] = massOrder1[j];
				massOrder1[j] = tempOrder;
			}
			if(mass2[i]>mass2[j]) {
				tempMass = mass2[i];
				mass2[i] = mass2[j];
				mass2[j] = tempMass;
				tempOrder = massOrder2[i];
				massOrder2[i] = massOrder2[j];
				massOrder2[j] = tempOrder;
			}
		}
	}
/*
	for(i=0;i<3;i++) {
		log_i("%d: massOrder1 = %d\tmassOrder2 = %d", i, massOrder1[i], massOrder2[i]);
	}
	for(i=0;i<3;i++) {
		log_i("%d: mass1 = %d\tmass2 = %d", i, mass1[i], mass2[i]);
	}
*/
	for(i=0;i<3;i++){
		if(triangle1.objMatch[massOrder1[i]] == triangle2.objMatch[massOrder2[i]]) {	
//			log_i("Match");
		}
		else {
			return 0;
		}
	}
	
	return 1;
}

void LogMatchSituation(const char* filename, const char *filenamePrimary, starMatch *match, starMatch tempMatch, bool rev) {
	strcpy(match->file, filename);
	strcpy(match->filePrimary, filenamePrimary);
	if(rev) {
		log_i("%s\t%d\t%d\t%f\t%f", filename, tempMatch.deltaX, tempMatch.deltaY, tempMatch.rotate, tempMatch.scale);
		match->scale = tempMatch.scale;
		match->rotate = tempMatch.rotate;
		match->deltaX = tempMatch.deltaX;
		match->deltaY = tempMatch.deltaY;
	}
	else
		log_i("%s %s", match->file, match->filePrimary);
}

void LogStarSituation(const char* filename, starMatch *match, int rev) {
	strcpy(match->file, filename);
	if(rev == -1)
		strcpy(match->filePrimary, "Cloud Detect!");
	else if(rev == -2)
		strcpy(match->filePrimary, "Star Not Enough!");
	else if(rev == -3)
		strcpy(match->filePrimary, "Star Line Detect!");
	else if(rev == -4)
		strcpy(match->filePrimary, "No Star Found!");
	else if(rev == -10)
		strcpy(match->filePrimary, "Img Empty!");
	log_i("%s %s", match->file, match->filePrimary);
	
}

//returns the number of star matches
int alignImages(starMatch *starMatches, const Mat& imgPrimary, const Mat& imgN, unsigned int& matchCount, float rssMetric, float angleMetric, float minAngle, unsigned int triNStars) {
	if(imgN.empty())
		return -2;

	int i,j,k,m;
	static imgObject *starObjects1 = NULL;
	imgObject *starObjects2 = NULL;
	unsigned int starCount1 = 0, starCount2 = 0;

	static triangleFeature *triangles1 = NULL;
	static unsigned int triangleCount1 = 0;
	triangleFeature *triangles2 = NULL;
	unsigned int triangleCount2 = 0;
	unsigned int triangleMatch1 = 0, triangleMatch2 = 0;
	
	possibleMatch *possibleMatches = NULL;
	unsigned int matches = 0;
	starMatch tempMatch = {0};
	starMatch fileMatch = {0};

	int alignResult = 0;
	int rev = 0;

	// log_i("Primary image:%s", primaryImage);
	// log_i("\nStarting the alignment process...");
	// log_i("Filename\tdeltaX\tdeltaY\trotation\tscale");		

	if (!imgPrimary.empty()) {
		xfree(starObjects1);
		rev = FindAndClassifyStars(imgPrimary, &starObjects1, &starCount1);	
		if(rev < 0) {
			// LogStarSituation(primaryImage, &fileMatch, rev);
			// SaveAlignmentToFile(alignFilename, &fileMatch);
			return -1;
		}

		xfree(triangles1);
		triangles1 = (triangleFeature*) calloc(lround(nchoosek(triNStars,3)), sizeof(triangleFeature));
		triangleCount1 = TriangulateFeatures(starObjects1, (triNStars > starCount1) ? starCount1 : triNStars, triangles1);
	}

	cv::Mat img_N_8b;
    imgN.convertTo(img_N_8b, CV_8UC3, 1 / 256.0);

	rev = FindAndClassifyStars(img_N_8b, &starObjects2, &starCount2);	
	if(rev < 0) {
		// LogStarSituation(file, &fileMatch, rev);
		// SaveAlignmentToFile(alignFilename, &fileMatch);
		return -2;
	}
	triangles2 = (triangleFeature*) calloc(lround(nchoosek(triNStars,3)), sizeof(triangleFeature));
	// log_i("\t\tTriangulating objects...");
	triangleCount2 = TriangulateFeatures(starObjects2, (triNStars > starCount2) ? starCount2 : triNStars, triangles2);
	possibleMatches = (possibleMatch*) calloc(triangleCount1, sizeof(possibleMatch));
	//If valid match found
	// log_i("\t\tMatching Triangulated objects...");
	if(MatchObjects(triangles1, triangles2, triangleCount1, triangleCount2, rssMetric, angleMetric, minAngle, &triangleMatch1, &triangleMatch2, possibleMatches, &matches)) {
		if(AuditMatches(possibleMatches, matches, &tempMatch)) {	
			// LogMatchSituation(file, primaryImage, starMatches, tempMatch, 1);
			log_i("%d\t%d\t%f\t%f", tempMatch.deltaX, tempMatch.deltaY, tempMatch.rotate, tempMatch.scale);
			starMatches->scale = tempMatch.scale;
			starMatches->rotate = tempMatch.rotate;
			starMatches->deltaX = tempMatch.deltaX;
			starMatches->deltaY = tempMatch.deltaY;
			memcpy(&fileMatch, starMatches, sizeof(starMatch));
			matchCount++;
			alignResult = 0;
		} else {
			// LogMatchSituation(file, "Match Failed!", &fileMatch, tempMatch, 0);
			log_i("Match Failed!");
			alignResult = -3;
		}
	} else {
		// LogMatchSituation(file, "No Possible Matches!", &fileMatch, tempMatch, 0);log_i("Match Failed!");
		log_i("No Possible Matches!");
		alignResult = -4;
	}

	// SaveAlignmentToFile(alignFilename, &fileMatch);

	xfree(starObjects2);
	xfree(triangles2);
	xfree(possibleMatches);
	
	return alignResult;
}

char AuditMatches(possibleMatch *possibleMatches, unsigned int matchCount, starMatch *finalMatch) {
	int i,j,k;	
	float scale1, scale2;
	float angle1, angle2;
	float deltaX1,deltaY1,deltaX2,deltaY2;
	unsigned int numMatches=1;
	unsigned int maxMatches=0;
	float avgScale=0, avgAngle=0, avgDeltaX=0, avgDeltaY=0;
	float sumScale=0, sumAngle=0, sumDeltaX=0, sumDeltaY=0;
	
	std::vector<float> scale_vec, angle_vec, deltaX_vec, deltaY_vec;
    for (int m = 0; m < matchCount; ++m) {
        float _scale, _angle, _deltaX, _deltaY;
        TransformMatch(possibleMatches[m].tri1, possibleMatches[m].tri2, &_scale, &_angle, &_deltaX, &_deltaY);
        scale_vec.push_back(_scale);
        angle_vec.push_back(_angle);
        deltaX_vec.push_back(_deltaX);
        deltaY_vec.push_back(_deltaY);
    }
    for (int i = 0; i < matchCount; ++i) {
        sumScale += scale_vec[i];
        sumAngle += angle_vec[i];
        sumDeltaX += deltaX_vec[i];
        sumDeltaY += deltaY_vec[i];
        for (int j = i + 1; j < matchCount; ++j) {
            if (fabs(scale_vec[i] - scale_vec[j]) < 0.01)
                if (fabs(angle_vec[i] - angle_vec[j]) < 0.01)
                    if (fabs(deltaX_vec[i] - deltaX_vec[j]) < 1)
                        if (fabs(deltaY_vec[i] - deltaY_vec[j]) < 1) {
                            numMatches++;
                            sumScale += scale_vec[j];
                            sumAngle += angle_vec[j];
                            sumDeltaX += deltaX_vec[j];
                            sumDeltaY += deltaY_vec[j];
                        }
        }
        if (numMatches > maxMatches) {
            avgScale = sumScale / (float)numMatches;
            avgAngle = sumAngle / (float)numMatches;
            avgDeltaX = sumDeltaX / (float)numMatches;
            avgDeltaY = sumDeltaY / (float)numMatches;
            maxMatches = numMatches;
        }
        numMatches = 1;
        sumScale = 0;
        sumAngle = 0;
        sumDeltaX = 0;
        sumDeltaY = 0;
    }
	// log_i("matches:%d scale:%f rot:%f dx:%f dy:%f",maxMatches, avgScale,avgAngle,avgDeltaX,avgDeltaY);

	finalMatch->file[0] = '\0';
	finalMatch->scale = avgScale;
	finalMatch->rotate = avgAngle;
	finalMatch->deltaX = roundf(avgDeltaX);
	finalMatch->deltaY = roundf(avgDeltaY);

	if(maxMatches < 3) {
		return 0;
	}
	else {
		return 1;
	}

}

char TransformMatch(triangleFeature *tri1, triangleFeature *tri2, float *scale, float *rot, float *deltaX, float *deltaY) {
	float	xNew, yNew;
	float	x1Temp, y1Temp, x2Temp, y2Temp;
	float	x1Center, y1Center, x2Center, y2Center;
	float	v1i,v1j,v2i,v2j;
	float 	mag2;	
	float	ang2;

	v1i = tri1->obj[1]->x - tri1->obj[0]->x;
	v1j = tri1->obj[1]->y - tri1->obj[0]->y;
	v2i = tri2->obj[tri1->objMatch[1]]->x - tri2->obj[tri1->objMatch[0]]->x;
	v2j = tri2->obj[tri1->objMatch[1]]->y - tri2->obj[tri1->objMatch[0]]->y;
		
	//Rotation
	*rot = acos(UnitDotProduct(v1i,v1j,v2i,v2j));
	*rot = (CrossProduct(v1i,v1j,v2i,v2j) < 0) ? -1*(*rot) : (*rot);	
	if(*rot != *rot) { *rot = 0.0; }	
			
	//Scale 
	*scale = sqrt(pow(v1i,2) + pow(v1j,2))/sqrt(pow(v2i,2) + pow(v2j,2));

	x1Center = (float)(tri1->obj[0]->img_width/2.0);
	y1Center = (float)(tri1->obj[0]->img_height/2.0);
	x2Center = (float)(tri2->obj[0]->img_width/2.0);
	y2Center = (float)(tri2->obj[0]->img_height/2.0);

	x1Temp = tri1->obj[0]->x;
	y1Temp = tri1->obj[0]->y;
	x2Temp = tri2->obj[tri1->objMatch[0]]->x;
	y2Temp = tri2->obj[tri1->objMatch[0]]->y;
			
	ang2 = atan2(x2Temp-x2Center,y2Temp-y2Center);
	mag2 = Mag(x2Temp - x2Center, y2Temp - y2Center);

	//Need to add back in the center of the image so the translation is with respect to the upper left corner or (0,0)
	xNew = (mag2*(*scale)*sin(ang2+(*rot))) + ((*scale)*x2Center);	
	yNew = (mag2*(*scale)*cos(ang2+(*rot))) + ((*scale)*y2Center);	
				
	//Translation
	*deltaX = -1*(xNew - (x1Temp));
	*deltaY = -1*(yNew - (y1Temp));

	return 1;
}

double factorial(unsigned int n) {
	unsigned int i;
	double temp;
	temp = 1;
	for(i=n; i>0; i--)
		temp*=i;
	return temp;	
}

double nchoosek(unsigned int n, unsigned int k) {
	return factorial(n)/(factorial(k)*factorial(n-k));
}
